//
// Created by bourne on 2024/4/16.
//

/***
 * 这段代码定义了一个名为 MultiMapManager 的C++类，
 * 它属于 fast_planner 命名空间。这个类旨在管理多张地图数据，
 * 通常用于无人机（或其他机器人）群体协作环境中，每个无人机都负责地图的一部分。
 *
 * MultiMapManager 负责同步和更新这些地图块，确保每个无人机都有最新的环境信息。
 * ***/

#ifndef SRC_MULTI_MAP_MANAGER_H
#define SRC_MULTI_MAP_MANAGER_H

#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>

#include <plan_env/ChunkStamps.h>
#include <plan_env/ChunkData.h>

#include <memory>
#include <random>
#include <vector>
#include <unordered_map>

using std::shared_ptr;
using std::vector;
using std::unordered_map;

namespace fast_planner {

class SDFMap;
class MapROS;

// A map chunk, the elementary exchange unit between robots
// MapChunk 结构体：代表一个地图块，包含地图块的索引、地址列表和占据信息。
//idx_：地图块的唯一标识符。
//voxel_adrs_：存储地图块中体素的地址。
//voxel_occ_：存储体素的占据状态。
//need_query_：标记地图块是否需要查询。
// 在体素刚刚进入到multi_map_chunks_的时候，
// 只有adr消息，还没有提取occ消息，所以是need_query
//empty_：标记地图块是否为空。
struct MapChunk {
  uint32_t idx_; // start from 1
  vector<uint32_t> voxel_adrs_;
  vector<uint8_t> voxel_occ_;

  bool need_query_;
  bool empty_;
};

// ChunksData 结构体：
//存储一组地图块的数据。
//chunks_：包含多个 MapChunk。
//idx_list_：地图块的索引列表。
struct ChunksData {
  vector<MapChunk> chunks_;
  vector<int> idx_list_; // start from 1
};

// ChunksBox 结构体：
//存储地图块的边界框。
//min_ 和 max_：边界框的最小和最大点。
//valid_：标记边界框是否有效。
struct ChunksBox {
  Eigen::Vector3d min_;
  Eigen::Vector3d max_;
  bool valid_;
};

class MultiMapManager {
public:
  // 构造函数和析构函数：初始化和清理 MultiMapManager 对象。
  MultiMapManager();
  ~MultiMapManager();

  // setMap：设置 SDFMap 对象的指针。
  // init：初始化 MultiMapManager。
  void setMap(SDFMap* map);
  void init();

  // updateMapChunk：更新地图块。
  void updateMapChunk(const vector<uint32_t>& adrs);
  // getChunkBoxes：获取所有地图块的边界框。
  void getChunkBoxes(
      vector<Eigen::Vector3d>& mins, vector<Eigen::Vector3d>& maxs, bool reset = true);
private:
  // sendChunks：发送地图块到其他无人机。
  void sendChunks(const int& chunk_drone_id, const int& to_drone_id, const vector<int>& idx_list);
  // getOccOfChunk：获取地图块的占据信息。
  void getOccOfChunk(const vector<uint32_t>& adrs, vector<uint8_t>& occs);
  // insertChunkToMap：将地图块插入到地图中。
  void insertChunkToMap(const MapChunk& chunk, const int& drone_id);
  // adrToIndex：将地址转换为索引。
  void adrToIndex(const uint32_t& adr, Eigen::Vector3i& idx);

  void stampTimerCallback(const ros::TimerEvent& e);
  void chunkTimerCallback(const ros::TimerEvent& e);
  void stampMsgCallback(const plan_env::ChunkStampsConstPtr& msg);
  void chunkCallback(const plan_env::ChunkDataConstPtr& msg);

  // Operations on the chunk idx list
  void findMissedChunkIds(
      const vector<int>& self_idx_list, const vector<int>& other_idx_list, vector<int>& miss_ids);
  bool findIntersect(
      const int& min1, const int& max1, const int& min2, const int& max2, int& minr, int& maxr);
  void mergeChunkIds(const vector<int>& list1, const vector<int>& list2, vector<int>& output);


  // data
  int drone_id_, map_num_;
  int vis_drone_id_; // only use for ground node
  int chunk_size_;

  SDFMap* map_;
  ros::NodeHandle node_;
  ros::Publisher stamp_pub_, chunk_pub_;
  ros::Publisher marker_pub_; // test to visualize the chunks
  ros::Subscriber stamp_sub_, chunk_sub_;
  ros::Timer stamp_timer_, chunk_timer_;

  // multi_map_chunks_ 这个数据结构存储了整个地图
  // vector中的元素数量是整个地图中飞机的数量, 然后每个元素分别区存储源于该飞机的地图块
  vector<ChunksData> multi_map_chunks_; // main map data

  // Buffer for chunks of this map
  vector<uint32_t> adr_buffer_;

  // 分别存储的是来自各个其他无人机的ChunkData数据
  // 直接将ros的ChunkData数据push进来，是一个数据处理的缓冲区
  vector<vector<plan_env::ChunkData>> chunk_buffer_;// Buffer for chunks of external map
  vector<unordered_map<int, char>> buffer_map_;  // Hash map to avoid repeated insertion of chunk msg

  vector<double> last_chunk_stamp_time_;

  // bounding box of map chunks of swarm
  vector<ChunksBox> chunk_boxes_;

  vector<Eigen::Vector3i> tmp_ids_;
  Eigen::Vector3d drone_pos_;

  friend SDFMap;
  friend MapROS;
};
} // namespace fast_planner

#endif //SRC_MULTI_MAP_MANAGER_H